/**
 * Author: luoqi
 * Created Date: 2025-11-08 11:03:21
 * Last Modified: 2025-11-20 00:54:35
 * Modified By: luoqi at <**@****>
 * Copyright (c) 2025 <*****>
 * Description:
 */

#include <stdlib.h>
#include <stdbool.h>
#include <bsp.h>
#include <cli.h>
#include <io_port.h>
#include <tim_port.h>
#include <dev/mc.h>
#include "ctrl.h"

typedef struct {
    float vloop_cycle; // velocity cycle time, unit: s
    float ploop_cycle; // position cycle time, unit: s
    float vset;  // velocity set point
    float pset;  // position set point
} CtrlParam;

static McObj m1;

static CtrlParam ctrl1 = {
    .vloop_cycle = 0.0002f,
    .ploop_cycle = 0.001f,
    .vset = 0.0f,
    .pset = 0.0f,
};

void ctrl_vset(float vset)
{ 
    ctrl1.vset = vset;
}

void ctrl_pset(float pset)
{
    ctrl1.pset = pset;
}

McMod ctrl_mod_get(void)
{
    return m1.mod;
}

float ctrl_pos_get(void)
{
    return m1.pos;
}

static void pwm_set(int pwm)
{
    if(pwm < -1000) {
        pwm = -1000;
    } else if(pwm > 1000) {
        pwm = 1000;
    }

    if(pwm < 0) {
        pwm = -pwm;
        tim_pwm_set(TIM_PORT_PWM1, PWM_CH1, pwm);
        tim_pwm_set(TIM_PORT_PWM1, PWM_CH2, 0);
    } else {
        tim_pwm_set(TIM_PORT_PWM1, PWM_CH1, 0);
        tim_pwm_set(TIM_PORT_PWM1, PWM_CH2, pwm);
    }
}

static void task_ctrl(void)
{
    if(m1.mod == MC_MOD_POS) {
        ctrl1.vset = mc_ploop(&m1, ctrl1.pset, ctrl1.ploop_cycle);
    }
}

static bool trigger = false;

void tim4_it_cb_overide()
{
    io_toggle(GPO_A15);
    trigger = true;
}

void task_immdiate_override(void)
{
    static float pos_k1 = 0.0f;

    if(!trigger) {
        return;
    }
    trigger = false;

    TimVal enc3;
    tim_get(TIM_PORT_ENC3, &enc3);
    int32_t epos = (enc3.count + enc3.overflow * 0xffff) % 16384;

    float pos = epos * 360.0f / 16384.0f;

    if(pos < 0.0f) {
        pos += 360.0f;
    }

    mc_pos_update(&m1, pos);

    float dpos = pos - pos_k1;
    if(dpos > 180.0f) {
        dpos -= 360.0f;
    } else if(dpos < -180.0f) {
        dpos += 360.0f;
    }

    float vel = dpos / ctrl1.vloop_cycle;
    pos_k1 = pos;
    mc_vel_update(&m1, vel);

    if(m1.mod == MC_MOD_VEL || m1.mod == MC_MOD_POS) {
        float v = mc_vloop(&m1, ctrl1.vset, ctrl1.vloop_cycle);
        
        pwm_set(v);
    }
}

static int cmd_mc_hdl(int argc, char **argv);

int ctrl_init(void)
{
    mc_init(&m1);
    mc_vpid_set(&m1, 1.2f, 0.03f, 0.0f, 20000);
    mc_vlpf_set(&m1, 200.0f, 1.0f / ctrl1.vloop_cycle);
    mc_ovlpf_set(&m1, 50.0f, 1.0f / ctrl1.vloop_cycle);
    mc_ppid_set(&m1, 15.0f, 0.01f, 0.0f, 1000);
    mc_ep_max_set(&m1, 45.0f);
    tim4_set_it_period(200);
    task_add("mc", task_ctrl, 1);
    cli_add("mc", cmd_mc_hdl, "motor control");
    return 0;
}

// debug command
static int args_vpid(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" kp: %f, ki: %f, kd: %f", m1.vpid.kp, m1.vpid.ki, m1.vpid.kd);
        return 0;
    } else if(argc == 4) {
        m1.vpid.kp = strtof(argv[1], NULL);
        m1.vpid.ki = strtof(argv[2], NULL);
        m1.vpid.kd = strtof(argv[3], NULL);
    } else {
        return -1;
    }
    return 0;
}

static int args_ppid(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" kp: %f, ki: %f, kd: %f", m1.ppid.kp, m1.ppid.ki, m1.ppid.kd);
        return 0;
    } else if(argc == 4) {
        m1.ppid.kp = strtof(argv[1], NULL);
        m1.ppid.ki = strtof(argv[2], NULL);
        m1.ppid.kd = strtof(argv[3], NULL);
    } else {
        return -1;
    }
    return 0;
}

static int args_mod(int argc, char **argv)
{
    if(argc != 2) {
        return -1;
    }
    if(ISARG(argv[1], "idle")) {
        m1.mod = MC_MOD_IDLE;
    } else if(ISARG(argv[1], "vel")) {
        m1.mod = MC_MOD_VEL;
    } else if(ISARG(argv[1], "pos")) {
        m1.mod = MC_MOD_POS;
    } else {
        return -1;
    }
    return 0;
}

static int args_vlpf(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" fc: %f", m1.vlpf.fc);
        return 0;
    } else if(argc == 2) {
        lpf_1st_fc_set(&m1.vlpf, strtof(argv[1], NULL));
    } else {
        return -1;
    }
    return 0;
}

static int args_ovlpf(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" fc: %f", m1.ovlpf.fc);
        return 0;
    } else if(argc == 2) {
        lpf_1st_fc_set(&m1.ovlpf, strtof(argv[1], NULL));
    } else {
        return -1;
    }
    return 0;
}



static int args_enable(int argc, char **argv)
{
    if(argc != 2) {
        return -1;
    }
    int ena = strtol(argv[1], NULL, 10);
    if(ena != 0 && ena != 1) {
        return -1;
    }
    if(ena == 0) {
        io_set(GPO_DRV_ENA, IO_LOW);
    } else {
        io_set(GPO_DRV_ENA, IO_HIGH);
    }
    return 0;
}

static int args_vset(int argc, char **argv)
{
    if(argc != 2) {
        return -1;
    }
    float v = strtof(argv[1], NULL);
    
    ctrl1.vset = v;
    return 0;
}

static int args_pset(int argc, char **argv)
{
    if(argc != 2) {
        return -1;
    }
    float p = strtof(argv[1], NULL);
    
    ctrl1.pset = p;
    return 0;
}

static int args_pwm(int argc, char **argv)
{
    if(argc != 2) {
        return -1;
    }
    int pwm = strtol(argv[1], NULL, 10);
    
    pwm_set(pwm);
    
    return 0;
}

static int args_epmax(int argc, char **argv)
{
    if(argc != 2) {
        return -1;
    }
    float ep_max = strtof(argv[1], NULL);
    if(ep_max <= 0.0f) {
        return -1;
    }

    mc_ep_max_set(&m1, ep_max);
    return 0;
}

static QCliArgsTable table[] = {
    {"vpid", args_vpid, "[<kp> <ki> <kd>]"},
    {"ppid", args_ppid, "[<kp> <ki> <kd>]"},
    {"mod", args_mod, "[<idle/vel/pos>]"},
    {"vlpf", args_vlpf, "[<fc>]"},
    {"ovlpf", args_ovlpf, "[<fc>]"},
    {"ena", args_enable, "[<0/1>]"},
    {"vset", args_vset, "[<v>]"},
    {"pset", args_pset, "[<p>]"},
    {"pwm", args_pwm, "[<pwm>]"},
    {"epmax", args_epmax, "[<ep_max>]"},
};

int cmd_mc_hdl(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" mod: %d\r\n", m1.mod);
        QSH(" pos: %f\r\n", m1.pos);
        QSH(" vel: %f\r\n", m1.vel);
        QSH(" vset: %f\r\n", ctrl1.vset);
        QSH(" pset: %f\r\n", ctrl1.pset);
        QSH(" vloop_cycle: %fms\r\n", ctrl1.vloop_cycle / 1000);
        QSH(" ploop_cycle: %fms\r\n", ctrl1.ploop_cycle / 1000);
        QSH(" ep_max: %f\r\n", m1.ep_max);
        return 0;
    }
    CLI_ARGS_EXEC(argc, argv, table);
}
